#include <stdio.h>
#include <errno.h>
#include <string.h>

#include <wiringPi.h>
#include <softPwm.h>
#include "dc_motor.h"


#define UP '1'
#define DOWN '2'
#define LEFT '3'
#define RIGHT '4'
#define STOP '5'
#define EXIT '9'



void set_direction (int dir);
int init_robot_car(void);
void show_menu(void);

int open_serial(char *dev_name, int baudrate);
char read_command ();
#define UART_DEV "/dev/ttyAMA0"
#define BAUDRATE 9600



int main ()
{
   int i, j ;
   int ret;
   
   pinMode(11,OUTPUT);
   
    if (open_serial(UART_DEV,BAUDRATE)<0)  {
    fprintf (stderr, "Unable to open serial device: %s\n", strerror (errno)) ;
    return -1 ;
   }

 if (wiringPiSetup () == -1)
   {
      fprintf (stdout, "oops: %s\n", strerror (errno)) ;
      return 1 ;
   }

    init_robot_car();
   set_direction(UP);

  //  read_command();    
   show_menu();

   	printf("\nByeBye\n\n ");		
  
}


void show_menu(void)
{
	char sel;
	
 	while(1)
		 {
		//	printf("****** iRobot Card Function Menu, please press (1~5), or 9 to exit! ********\n ");		
	#if 1
	 sel=read_command();

	
	#else 
			printf("1:forward 2:back 3:Left up 4:right 5:stop 9: exit \n ");
			scanf("%c",&sel);	
			while (getchar()!='\n') 
					continue;	
  #endif					
			switch(sel)
				{
					
					case UP:
						set_direction(UP);
						break;
					case DOWN:
						set_direction(DOWN);
						break;
					case LEFT:
						set_direction(LEFT);
						break;
					case RIGHT:
						set_direction(RIGHT);
						break;
					case STOP:
						set_direction(STOP);
						break;	
					case EXIT:
						set_direction(STOP);
						return ;												
						break;						
	      }

  	}
}



int init_robot_car(void)
{
 
   
   pinMode(MOTOR_1_PWM, OUTPUT);
   pinMode(MOTOR_2_PWM, OUTPUT);
   pinMode(MOTOR_3_PWM, OUTPUT);
   pinMode(MOTOR_4_PWM, OUTPUT);
   
   digitalWrite(MOTOR_1_PWM, 0);
   digitalWrite(MOTOR_2_PWM, 0);
   digitalWrite(MOTOR_3_PWM, 0);
   digitalWrite(MOTOR_4_PWM, 0);
 
   DCMotorInit();

   
     digitalWrite(MOTOR_1_PWM, 1);
     digitalWrite(MOTOR_2_PWM, 1);
     digitalWrite(MOTOR_3_PWM, 1);
     digitalWrite(MOTOR_4_PWM, 1);
  

}   





void set_direction (int dir)
{
	int i;
	 printf("%s,%c\n",__func__,dir);
 switch (dir) {
 
  case UP:
   //motor.setSpeed(255); //min 0~255 max
  // motor_l.setSpeed(255);
   DCMotorRun(1, FORWARD);
   DCMotorRun(2, FORWARD);
   DCMotorRun(3, FORWARD);
   DCMotorRun(4, FORWARD);
   DCMotorSetSpeed(1,100);
   DCMotorSetSpeed(2,100);
   DCMotorSetSpeed(3,100);
   DCMotorSetSpeed(4,100);   
   
    break;
 
   case DOWN:
 
   DCMotorRun(1, BACKWARD);
   DCMotorRun(2, BACKWARD);
   DCMotorRun(3, BACKWARD);
   DCMotorRun(4, BACKWARD);
   DCMotorSetSpeed(1,100);
   DCMotorSetSpeed(2,100);
   DCMotorSetSpeed(3,100);
   DCMotorSetSpeed(4,100);    
    break; 
 
   case LEFT:
   DCMotorSetSpeed(1,100);
   DCMotorSetSpeed(2,25);
   DCMotorSetSpeed(3,25);
   DCMotorSetSpeed(4,100);   

    break;
    
   case RIGHT:
   DCMotorSetSpeed(1,25);
   DCMotorSetSpeed(2,100);
   DCMotorSetSpeed(3,100);
   DCMotorSetSpeed(4,25); 


    break;   
  case STOP: 
   DCMotorRun(1, RELEASE);
   DCMotorRun(2, RELEASE);
   DCMotorRun(3, RELEASE);
   DCMotorRun(4, RELEASE);
    
    break;
 } 
}

